Moving state calculating method and moving state calculating device

ABSTRACT

A first moving state at a given time including any of a position, a velocity, and a moving direction of a moving object moving in a space is estimated. A constraint condition for calculating a second moving state at the given time is set using the first moving state and a previously-calculated second moving state including any of a position, a velocity, and a moving direction of the moving object moving in the space. The first moving state is corrected using the constraint condition to calculate the second moving state at the given time.

CROSS-REFERENCE

This application claims priority to Japanese Patent Application No.2012-193170, filed Sep. 3, 2012, the entirety of which is herebyincorporated by reference.

BACKGROUND

1. Technical Field

The present invention relates to a method of calculating a moving stateof a moving object and the like.

2. Related Art

Techniques of estimating moving states including quantities such as aposition, a velocity, and a moving direction of a moving object movingin a movement space using a state estimating technique in a state spacehave been invented. The Kalman filter is widely known as the stateestimating technique. For example, JP-A-2010-266468 discloses atechnique of estimating a position of a moving object using the Kalmanfilter.

In the estimation of a moving state using the Kalman filter, a movingstate of a moving object changing from time to time is sequentiallyestimated using observation information including an error. Referenceinformation detected, for example, by an external sensor (a sensor suchas a global positioning system (GPS) sensor, an inertial sensor, or ageometric sensor, which is hereinafter comprehensively referred to as a“reference sensor”) is used as the observation information.

However, the above-mentioned techniques have a problem in thatestimation accuracy of a moving state depends on only detection accuracyof the reference sensor. That is, when the detection accuracy of thereference sensor is low, inaccurate reference information is used as theobservation information for calculation of the Kalman filter and thusthere is a problem in that accuracy of the estimated moving state islowered due to inclination to the observation information.

SUMMARY

An advantage of some aspects of the invention is that it provides a newtechnique of correctly calculating a moving state of a moving object.

A first aspect of the invention is directed to a moving statecalculating method including: estimating a first moving state at a giventime including any of a position, a velocity, and a moving direction ofa moving object moving in a space; setting a constraint condition forcalculating a second moving state at the given time using the firstmoving state and a previously-calculated second moving state includingany of a position, a velocity, and a moving direction of the movingobject moving in the space; and correcting the first moving state usingthe constraint condition to calculate the second moving state at thegiven time.

As another aspect of the invention, the aspect of the invention may beconfigured as a moving state calculating device including: an estimationunit that estimates a first moving state at a given time including anyof a position, a velocity, and a moving direction of a moving objectmoving in a space; a setting unit that sets a constraint condition forcalculating a second moving state at the given time using the firstmoving state and a previously-calculated second moving state includingany of a position, a velocity, and a moving direction of the movingobject moving in the space; and a correction unit that corrects thefirst moving state using the constraint condition to calculate thesecond moving state at the given time.

According to the first aspect and the like, a first moving state at agiven time including any of a position, a velocity, and a movingdirection of a moving object moving in a movement space (space) isestimated. A constraint condition for calculating a second moving stateat the given time is set using the first moving state and apreviously-calculated second moving state including any of a position, avelocity, and a moving direction of the moving object moving in themovement space. The first moving state is corrected using the constraintcondition to calculate the second moving state at the given time. Theestimating of the first moving state can be implemented by the use of aknown state estimating technique (for example, Kalman filter). Thesecond moving state (new second moving state) at the given time iscalculated by correcting the first moving state at the given time usinga predetermined constraint condition as well as performing theestimation of the first moving state. As a result, it is possible tocorrectly calculate the moving state of the moving state.

As a second aspect of the invention, the moving state calculating methodaccording to the first aspect may be configured such that the setting ofthe constraint condition includes setting the constraint condition so asto follow a state variation tendency of the previously-calculated secondmoving state.

According to the second aspect, the constraint condition is set to causethe first moving state to follow the tendency of the state variation ofthe previously-calculated second moving state.

As a third aspect of the invention, as the constraint condition of thiscase, for example, the constraint condition may be set to continue torectilinearly move when the state variation of the previously-calculatedsecond moving state indicates a rectilinear moving tendency.

According to the third aspect, it is possible to calculate the secondmoving state at the given time by correcting the first moving state toexhibit the rectilinear moving tendency.

As a fourth aspect of the invention, for example, the constraintcondition may be set to continue to turn when the state variation of thepreviously-calculated second moving state indicates a turning tendency.

According to the fourth aspect, it is possible to calculate the secondmoving state at the given time by correcting the first moving state toexhibit the turning tendency.

as a fifth aspect of the invention, the moving state calculating methodaccording to any of the second to fourth aspects may be configured suchthat the setting of the constraint condition includes calculating aparameter indicating a degree of adapting the first moving state to thetendency of the state variation of the previously-calculated secondmoving state, and the correcting of the first moving state includescausing the first moving state to get close to a state in which theconstraint condition is satisfied using the parameter.

According to the fifth aspect, the parameter indicating a degree ofadapting the first moving state to the tendency of the state variationof the previously-calculated second moving state is calculated. Thefirst moving state is made to get close to a state where the constraintcondition is satisfied using the parameter. Accordingly, it is possibleto calculate the second moving state at the given time by appropriatelycorrecting the first moving state.

as a sixth aspect of the invention of the invention, the moving statecalculating method according to the fifth aspect may be configured suchthat the setting of the constraint condition includes determining areference moving direction using the previously-calculated second movingstate, and the calculating of the parameter includes calculating theparameter on the basis of a difference between the moving directionincluded in the first moving state and the reference moving direction.

According to the sixth aspect, the reference moving direction isdetermined using the previously-calculated second moving state. Anappropriately value can be set as the parameter indicating the degree ofadapting the first moving state to the tendency of the state variationof the previously-calculated second moving state on the basis of thedifference between the moving direction included in the first movingstate and the reference moving direction.

BRIEF DESCRIPTION OF THE DRAWINGS

The invention will be described with reference to the accompanyingdrawings, wherein like numbers reference like elements.

FIG. 1 is a flowchart illustrating a process flow of estimating a movingstate.

FIG. 2A is a diagram illustrating a rectilinear moving condition.

FIG. 2B is a diagram illustrating a method of setting an adaptationparameter.

FIG. 3A is a diagram illustrating a smoothing condition.

FIG. 3B is a diagram illustrating a method of setting an adaptationparameter.

FIG. 4 is a diagram illustrating an example of a functionalconfiguration of a position calculating device.

FIG. 5 is a flowchart illustrating a flow of a position calculatingprocess.

FIG. 6A is a diagram illustrating an example of a position calculationresult when the rectilinear moving condition is employed.

FIG. 6B is a diagram illustrating an example of a position calculationresult when the smoothing condition is employed.

DESCRIPTION OF EXEMPLARY EMBODIMENTS 1. Principle 1-1. General

First, parameters or the like will be defined. In this exemplaryembodiment, a movement space of a moving object is defined as a statespace. A moving state in the movement space is marked using “x” and amoving state at time “k” is marked by “x_(k)”. An observation value ismarked using “y” and the observation value acquired at time “k” ismarked by “y_(k)”. A set of observation values including the observationvalues up to time k is defined as an observation signal and is marked by“Y_(k)={y₀, y₁, . . . , y_(k)}”.

When the state estimating technique known in the related art is used toestimate a moving state, the moving state is sequentially estimated byrepeating prediction of the moving state and correction of the movingstate. In the prediction of a moving state, a current moving state ispredicted from a previously-estimated moving state. This means that thefuture is inferred from previous observation. The prediction of a movingstate x_(k) at time k can be expressed by Expression 1 using theconditional probability density function of the moving state x_(k) whenan observation signal Y_(k-1)={y₀, y₁, . . . , y_(k-1)} up to time k−1is given.p(x _(k) |Y _(k-1))=∫p(x _(k) |x _(k-1))p(x _(k-1) |Y _(k-1))dx_(k-1)  (1)

In the correction of a moving state, the predicted value acquired fromthe prediction of the moving state is corrected using a newly acquiredobservation value. Specifically, the moving state x_(k) is correctedusing the observation value y_(k) newly acquired at time k. Since theobservation value y_(k) is acquired in a state where the observationsignal Y_(k-1)={y₀, y₁, . . . , y_(k-1)} is given already, theobservation signal is changed to Y_(k)={y₀, y₁, . . . , y_(k)}. In thiscase, the correction of the moving state x_(k) can be expressed byExpression 2 using the conditional probability density function of themoving state x_(k) when the observation signal Y_(k) is given.

$\begin{matrix}{{p\left( {x_{k}❘Y_{k}} \right)} = \frac{{p\left( {y_{k}❘x_{k}} \right)}{p\left( {x_{k}❘Y_{k - 1}} \right)}}{\int{{p\left( {y_{k}❘x_{k}} \right)}{p\left( {x_{k}❘Y_{k - 1}} \right)}{\mathbb{d}x_{k}}}}} & (2)\end{matrix}$

The operation of estimating a moving state up to now is only applicationof the state estimating technique known in the related art. In thisexemplary embodiment, a constraint condition for newly calculating asecond moving state is set using the moving state (hereinafter, referredto as “first moving state”) estimated by performing the operation ofestimating a moving state and the previously-calculated moving state(hereinafter, referred to as “second moving state”). The first movingstate is corrected using the set constraint condition, whereby the newsecond moving state is calculated. This is the most important feature ofthis exemplary embodiment.

In order to introduce the constraint condition, a parameter representingthe constraint condition is introduced as a parameter of the conditionalprobability density function of the moving state x_(k). Although detailsof the constraint condition will be described later, a parameter calledconstraint condition definition value “d” is introduced as a parameternecessary for defining the constraint condition. The constraintcondition definition value at time “k” is marked by “d_(k)”. A set ofconstraint condition definition values including the constraintcondition definition values up to time k is referred to as a constraintcondition signal and is marked by “D_(k)={d₀, d₁, . . . , d_(k)}”.

FIG. 1 is a flowchart illustrating a process flow (moving statecalculating method) of calculating a moving state in this exemplaryembodiment.

First, a moving state estimating process is performed (step A1). In themoving state estimating process, as described above, two processes ofthe prediction of a moving state (step A3) and the correction of amoving state (step A5) are performed.

In step A3, the moving state x at the current time is predicted usingthe previously-calculated second moving state. Specifically, theprediction of the moving state x_(k) at time k can be expressed byExpression 3 using the conditional probability density function of themoving state x_(k) when the observation signal Y_(k-1)={y₀, y₁, . . . ,y_(k-1)} up to time k−1 and the constraint condition signal D_(k-1)={d₀,d₁, . . . , d_(k-1)} up to time k−1 are given.p(x _(k) |Y _(k-1) ,D _(k-1))=∫p(x _(k) |x _(k-1))p(x _(k-1) |Y _(k-1),D _(k-1))dx _(k-1)  (3)

In step A5, the moving state x_(k) predicted in step A3 is correctedusing a newly acquired observation value y_(k). By newly acquiring theobservation value y_(k) in a state where the observation signalY_(k-1)={y₀, y₁, . . . , y_(k-1)} up to time k−1 is given, theobservation signal becomes observation signal Y_(k)={y₀, y₁, . . . ,y_(k)}. In this case, the correction of the moving state x_(k) can beexpressed by Expression 4 using the conditional probability densityfunction of the moving state x_(k) when the observation signal y_(k) andthe constraint condition signal D_(k-1) up to time k−1 are given.

$\begin{matrix}{{p\left( {{x_{k}❘Y_{k}},D_{k - 1}} \right)} = \frac{{p\left( {y_{k}❘x_{k}} \right)}{p\left( {{x_{k}❘Y_{k - 1}},D_{k - 1}} \right)}}{\int{{p\left( {y_{k}❘x_{k}} \right)}{p\left( {{x_{k}❘Y_{k - 1}},D_{k - 1}} \right)}{\mathbb{d}x_{k}}}}} & (4)\end{matrix}$

After the moving state estimating process of step A1 is performed, it isdetermined whether the constraint condition should be applied (step A7).When it is determined that the constraint condition should not beapplied (NO in step A7), the moving state (first moving state) estimatedthrough the moving state estimating process is calculated as a newmoving state (second moving state) (step A17). Then, the process flowgoes to step A19.

On the other hand, when it is determined that the constraint conditionshould be applied (YES in step A7), the moving state correcting processis performed (step A9). In the moving state correcting process, first,the constraint condition is set (step A11). By newly setting theconstraint condition at time k in a state where the constraint conditionsignal D_(k-1)={d₀, d₁, . . . , d_(k-1)} up to time k−1 is given, theconstraint condition signal becomes D_(k)={d₀, d₁, . . . , d_(k)}.

Subsequently, the moving state (first moving state) estimated throughthe moving state estimating process is corrected using the constraintcondition set in step A11 (step A13). The correction of the first movingstate can be expressed by Expression 5 using the conditional probabilitydensity function of the moving state x_(k) when the observation signalY_(k) and the constraint condition signal D_(k) are given.

$\begin{matrix}{{p\left( {{x_{k}❘Y_{k}},D_{k}} \right)} = \frac{{p\left( {d_{k}❘x_{k}} \right)}{p\left( {{x_{k}❘Y_{k}},D_{k - 1}} \right)}}{\int{{p\left( {d_{k}❘x_{k}} \right)}{p\left( {{x_{k}❘Y_{k}},D_{k - 1}} \right)}{\mathbb{d}x_{k}}}}} & (5)\end{matrix}$

When the moving state correcting process has been performed, thecorrected moving state (first moving state) is calculated as a newmoving state (second moving state) (step A15).

After performing the process of step A15 or A17, it is determinedwhether the calculation of the moving state should end (step A19). Whenit is determined that the calculation of the moving state shouldcontinue to be performed (NO in step A19), the process flow goes back tostep A1. When it is determined that the calculation of the moving stateshould end (YES in step A19), the process flow of the calculation of themoving state ends.

1-2. Particulars

The processes performed in the steps of FIG. 1 will be described belowin detail.

1-2-1. Moving State Estimating Process

In FIG. 1, the prediction and the correction of a moving state in themoving state estimating process of step A1 are expressed using theconditional probability density function of the moving state x_(k). Themoving state estimating process can be realized as a moving stateestimating process using the Kalman filter by introducing Gaussianassumptions into Expression 3 representing the prediction of a movingstate and Expression 4 representing the correction of a moving state.

The state equation in the Kalman filter can be expressed by Expression6.x _(k) =F _(k) x _(k-1) +w _(k)  (6)

In Expression 6, “F_(k)” represents a time-transition linear model ofthe moving state x. “w_(k)” represents the system noise and complieswith the multivariable normal distribution of a covariance Q_(k) and azero average, that is, “w_(k)˜N(0, Q_(k))”.

The observation equation in the Kalman filter is expressed by Expression7.y _(k) =H _(k) x _(k) +v _(k)  (7)

In Expression 7, “H_(k)” represents an observation model of linearlymapping the state space onto an observation space. “v_(k)” representsthe noise of an observation value and complies with the multivariablenormal distribution of a covariance Rk and a zero average, that is,“v_(k)˜N(0, Rk)”.

In the moving state estimating process using the Kalman filter, theprediction and the correction of the moving state x are performed on thebasis of the state equation expressed by Expression 6 and theobservation equation expressed by Expression 7. Since the operationalequations related to the prediction and the correction of the Kalmanfilter are known in the related art, they will not be described herein.

1-2-2. Moving State Correcting Process

In the moving state correcting process, the first moving state iscorrected using the constraint condition. In this exemplary embodiment,the constraint condition is described using the constraint equationexpressed by Expression 8.d _(k) =g _(k)(x _(k))+ε_(k)  (8)

In Expression 8, “d_(k)” represents the constraint condition definitionvalue and “g_(k)(•)” represents a model function (hereinafter, referredto as “constraint condition model function”) for giving a constraintcondition. “ε_(k)” represents an error term of the constraint condition.In this exemplary embodiment, it is assumed that the error term “ε_(k)”of the constraint condition complies with the multivariable normaldistribution of a covariance S_(k) and a zero average, that is,“ε_(k)˜N(0, S_(k))”.

The constraint condition model function g_(k)(•) is individually setdepending on the type of the constraint condition to be set. A nonlinearfunction as well as a linear function can be applied as the constraintcondition model function g_(k)(•). When a nonlinear function is set asthe model function, it is difficult to perform calculation using theform of the model function without any change and thus the modelfunction is linearized.

The moving state estimated in the moving state estimating process attime k is marked by “x⁺ _(k)”. At this time, by Taylor developing theconstraint condition model function g_(k)(•) up to the first-order termin the vicinity of the known moving state x⁺ _(k), Expression 9 isobtained.

$\begin{matrix}{{{g_{k}\left( x_{k} \right)} \approx {{g_{k}\left( x_{k}^{+} \right)} + {G_{k}\left( {x_{k} - x_{k}^{+}} \right)}}}{{{provided}\mspace{14mu}{that}},{G_{k} = {\frac{\partial{g_{k}\left( x_{k} \right)}}{\partial x_{k}}❘_{x_{k} = x_{k}^{+}}}}}} & (9)\end{matrix}$

Here, “G_(k)” represents a matrix (hereinafter, referred to as“constraint matrix”) obtained by partial differentiating g_(k)(x_(k))with x_(k).

Here, by substituting Expression 9 for Expression 8, Expression 10 isderived.

$\begin{matrix}{{{d_{k} \approx {{g_{k}\left( x_{k}^{+} \right)} + {G_{k}\left( {x_{k} - x_{k}^{+}} \right)} + ɛ_{k}}} = {{G_{k}x_{k}} + u_{k} + ɛ_{k}}}{{{provided}\mspace{14mu}{that}},{u_{k} = {{g_{k}\left( x_{k}^{+} \right)} - {G_{k}x_{k}^{+}}}}}} & (10)\end{matrix}$

Expression 10 has the same form as the state equation receiving anexternal input u_(k) in the Kalman filter. Therefore, a correctingoperational expression for correcting the moving state x_(k) is derivedfrom Expression 10 on the basis of the Kalman filter.

As a result, the correcting operational expressions shown in Expressions11 to 13 are derived.L _(k) =P _(k) ⁺ G _(k) ^(T)(G _(k) P _(k) ⁺ G _(k) ^(T) +S_(k))⁻¹  (11)x _(k) =x _(k) ⁺ +L _(k)(d _(k) −g _(k)(x _(k) ⁺))  (12)P _(k)=(I−L _(k) G _(k))P _(k) ⁺  (13)

Here, “L_(k)” represents a correction coefficient (gain) for correctingthe moving state x_(k) and “P_(k)” represents the error covariance ofthe moving state x_(k). “I” represents a unit matrix. The superscript“+” means a value estimated through the moving state estimating process.That is, “x⁺ _(k)” means the moving state estimated through the movingstate estimating process and “P⁺ _(k)” means the error covarianceestimated through the moving state estimating process.

In step A13 of the moving state correcting process shown in FIG. 1, thefirst moving state estimated through the moving state estimating processof step A1 is corrected on the basis of the correcting operationalexpressions expressed by Expressions 11 to 13.

1-2-3. Setting of Constraint Condition

In this exemplary embodiment, conditions used to correct the firstmoving state to comply with the tendency of the state variation of thepreviously-calculated second moving state are set as the constraintcondition. In order to correct the first moving state on the basis ofExpressions 11 to 13, it is necessary to determine the constraintcondition model function g_(k)(x_(k)), the constraint conditiondefinition value d_(k), and the covariance S_(k).

The constraint equation (Expression 8) representing the constraintcondition is determined by the constraint condition model functiong_(k)(x_(k)) and constraint condition definition value d_(k). The degreeof adapting the first moving state estimated in the moving stateestimating process to the tendency of the state variation of thepreviously-calculated second moving state is determined by thecovariance S_(k).

When the covariance S_(k) increases, an action of lowering the degree ofadapting the first moving state to the tendency of the state variationof the previously-calculated second moving state occurs. This is becausewhen the covariance S_(k) in Expression 11 gets close to “+∞”(S_(k)→+∞), the correction coefficient L_(k) gets close to “0”. Then,Expression 12 is changed to “x_(k)=x⁺ _(k)”. This means that the firstmoving state estimated in the moving state estimating process ismaintained without any change and the action of the constraint conditionis set to zero. On the contrary, When the covariance S_(k) decreases, anaction of raising the degree of adapting the first moving state to thetendency of the state variation of the previously-calculated secondmoving state occurs. That is, it is possible to enhance the action ofthe constraint condition to correct the moving state.

Accordingly, the covariance S_(k) can be said to be a parameterrepresenting the degree of adapting the first moving state estimated inthe moving state estimating process to the tendency of the statevariation of the previously-calculated second moving state. Therefore,the covariance S_(k) is referred to as “adaptation parameter” in thefollowing description.

A specific example of the constraint condition will be described below.The example illustrates the case in which the moving state x_(k) isassumed as a velocity vector v_(k) of a moving object (for example,automobile) and the constraint condition for the velocity vector v_(k)is set. Since the velocity vector v_(k) represents the velocity and themoving direction of the moving object, the velocity and the movingdirection of the moving object is determined as the moving state x_(k)in this example.

FIGS. 2A and 2B are diagrams illustrating a rectilinear moving conditionwhich is an example of the constraint condition. The rectilinear movingcondition is a condition used to correct the first moving state so as tocontinue to rectilinearly move when the state variation of thepreviously-calculated second moving state indicates a rectilinear movingtendency. For example, when an automobile travels in a straight road(such as an expressway), the moving direction of the automobile is arectilinear direction parallel to the traveling direction. Therefore,the velocity vector v_(k) calculated as the moving state x_(k) needs torepresent a rectilinear moving tendency. As a result, a constraint onthe moving direction is given so as to calculate the velocity vectorv_(k) indicating the rectilinear moving condition.

FIG. 2A schematically illustrates a velocity vector of an automobile attimes k−4 to k. A velocity vector having a black circle at a start pointrepresents a previously-calculated velocity vector. A velocity vectorhaving a white circle at a start point represents a newest velocityvector estimated in the moving state estimating process.

A state is considered in which the velocity vector v_(k) at time k isestimated through the moving state estimating process. At this time, forexample, an average velocity vector v_(ave) is calculated by averagingthe velocity vectors corresponding to a predetermined period of time(for example, previous three time units) going back to the past fromtime k. A function of calculating an inner product of a unit vectorv_(k)/∥v_(k)∥ of the velocity vector estimated at time k and a unitvector v_(ave)/∥v_(ave)∥ of the average velocity vector is set as theconstraint condition model function g_(k)(•).

Specifically, for example, a constraint condition model functiong_(k)(v_(k)) expressed by Expression 14 is set.

$\begin{matrix}{{g_{k}\left( {\overset{\rightarrow}{v}}_{k} \right)} = {{\frac{{\overset{\rightarrow}{v}}_{k}}{{\overset{\rightarrow}{v}}_{k}} \cdot \frac{{\overset{\rightarrow}{v}}_{ave}}{{\overset{\rightarrow}{v}}_{ave}}} = {\frac{{{\overset{\rightarrow}{v}}_{k}}{{\overset{\rightarrow}{v}}_{ave}}\cos\;\theta_{k}}{{{\overset{\rightarrow}{v}}_{k}}{{\overset{\rightarrow}{v}}_{ave}}} = {\cos\;\theta_{k}}}}} & (14)\end{matrix}$

Here, “θ_(k)” represents an angle formed by the velocity vector v_(k)and the average velocity vector v_(ave).

In order to align the direction of the velocity vector v_(k) and thedirection of the average velocity vector v_(ave) with each other, aconstraint for setting the angle θ_(k) formed by the velocity vectorv_(k) and the average velocity vector v_(ave) to 0 degree, that is, aconstraint for setting the inner product calculated by Expression 14 to“1”, can be given. Therefore, the constraint condition definition valued_(k) in the constraint equation of Expression 8 is set to “1”(d_(k)=1).

FIG. 2B is a diagram illustrating a method of setting the adaptationparameter S_(k) in this case. In FIG. 2B, the horizontal axis representsthe inner product cos θ_(k) calculated using Expression 14, and thevertical axis represents the set value of the adaptation parameterS_(k). When the direction of the velocity vector v_(k) and the directionof the average velocity vector v_(ave) agree to each other, “cosθ_(k)=1” is obtained. In this case, it is not necessary to give anyconstraint on the velocity vector v_(k). Therefore, in order to minimizethe action of the constraint, the set value of the adaptation parameterS_(k) is set to a given maximum definition value S_(max).

On the contrary, when the direction of the velocity vector v_(k) and thedirection of the average velocity vector v_(ave) are reverse, “cosθ_(k)=−1” is obtained. In this case, in order to align the direction ofthe velocity vector v_(k) with the direction of the average velocityvector v_(ave), it is necessary to maximize the strength of theconstraint. Therefore, in order to maximize the action of theconstraint, the set value of the adaptation parameter S_(k) is set to agiven minimum definition value S_(min).

In a range of −1<cos θ_(k)<1″, the action of the constraint can be madeto decrease with a decrease in θ_(k). Accordingly, for example, as shownin FIG. 2B, the adaptation parameter S_(k) is calculated depending on alinear increasing function connecting the minimum definition valueS_(min) and the maximum definition value S_(max).

The function related to the calculation of the adaptation parameterS_(k) in this case does not have to employ the function shown in FIG.2B, but may employ, for example, a nonlinear increasing function.

In the method of setting the adaptation parameter S_(k), the averagevelocity vector is determined using the previously-calculated velocityvectors, and the value of the adaptation parameter S_(k) is calculatedusing the inner product of the average velocity vector and the estimatednewest velocity vector. The inner product of vectors is equivalent tothe angle formed by the vectors. Therefore, in the above-mentionedmethod, the direction of the average velocity vector is determined to bea reference moving direction and the adaptation parameter S_(k) iscalculated on the basis of the difference between the estimated newestmoving direction and the reference moving direction.

In the above-mentioned example, the function of calculating the innerproduct of the average velocity vector v_(ave) and the newest velocityvector v_(k) is defined as the constraint condition model function, buta function of calculating an inner product of the estimated newestvelocity vector v_(k) and the previously-calculated velocity vectorv_(k-1) may be defined as the constraint condition model function.

FIGS. 3A and 3B are diagrams illustrating a smoothing condition which isanother example of the constraint condition. The smoothing condition isa condition used to correct the first moving state so as to continue toturn when the state variation of the previously-calculated second movingstate indicates a turning tendency. For example, when an automobileturns along a curve, the smoothing condition is a condition for giving aconstraint on the velocity vector so as to calculate a velocity vectoralong the curved line of the curve.

FIG. 3A schematically illustrates the velocity vector of an automobileat times k−3 to k. The point of view of the drawing is the same as FIG.2A. In the smoothing condition, a condition for making the angle θformed by neighboring velocity vectors constant is given as theconstraint condition to the velocity vectors acquired in time series.

The angle formed by the velocity vector v_(k) at time k and the velocityvector v_(k-1) at time k−1 is marked by θ_(k). The angle θ_(k) iscalculated using the inner product and the outer product of the velocityvector v_(k) and the velocity vector v_(k-1). The magnitude of the angleθ_(k) is calculated using the inner product and the relative rotationdirection of the neighboring velocity vectors is calculated using theouter product. The plus or minus sign of the angle θ_(k) variesdepending on the relative rotation direction of the neighboring velocityvectors.

An average angle θ_(ave) is calculated by averaging the angles formed bythe neighboring velocity vectors in a predetermined previous period oftime (for example, previous three time units). A model function, whichis expressed by Expression 15, for calculating an angle differenceΔθ_(k) between the angle θ_(k) at time k and the average angle θ_(ave)is set as the constraint condition model function g_(k)(•).g _(k)({right arrow over (v)} _(k))−θ_(k)−θ_(ave)=Δθ_(k)  (15)

In the smoothing condition, in order to make the angle formed by theneighboring velocity vectors constant, a constraint for setting theangle difference Δθ_(k) calculated by Expression 15 has only to begiven. Therefore, the constraint condition definition value d_(k) in theconstraint equation of Expression 8 is set to “0” (d_(k)=0).

FIG. 3B is a diagram illustrating the method of setting the adaptationparameter S_(k) in this case. In FIG. 3B, the horizontal axis representsthe absolute value |Δθ_(k)| of the angle difference Δθ_(k) calculated byExpression 15 and the vertical axis represents the set value of theadaptation parameter S_(k). As described above, in the smoothingcondition, it is ideal that the angle difference Δθ_(k) is zero.Therefore, in order to minimize the action of the constraint, the setvalue of the adaptation parameter S_(k) when “Δθ_(k)=0” is set to thegiven maximum definition value S_(max).

The set value of the adaptation parameter S_(k) is made to decrease toenhance the action of the constraint as the value |Δθ_(k)| increases.Specifically, for example, as shown in FIG. 3B, the adaptation parameterS_(k) is calculated on the basis of a nonlinear decreasing functiongetting closer to the given minimum definition value S_(min) from themaximum definition value S_(max) with the increase in |Δθ_(k)|.

The function for calculating the adaptation parameter S_(k) in this casedoes not have to employ the function shown in FIG. 3B, but may employ,for example, a linear decreasing function.

In the example, the function of calculating the angle difference betweenthe newest angle θ_(k) and the average angle θ_(ave) in a predeterminedperiod of time is defined as the constraint condition model function, afunction of calculating an angle difference between the newest angleθ_(k) and the previously-calculated angle θ_(k-1) may be set as theconstraint condition model function instead.

The rectilinear moving condition and the smoothing condition areexamples of the constraint condition. These constraint conditions can beapplied substantially in the same way when the position of p_(k) of amoving object should be calculated as well as when the velocity vectorv_(k) of a moving object should be calculated. Since the variation inposition per unit time corresponds to the velocity, the model functionrelated to the rectilinear moving condition or the smoothing conditionexpressed by Expression 14 or 15 can be similarly formulated using adifference p_(k)−p_(k-1) between position vectors instead of thevelocity vector v_(k). By using the model function formulated in thisway, it is possible to perform an operation of correcting the positionp_(k) using the constraint condition with the position p_(k) of a movingobject as the state parameter.

2. Example

An example of a moving state calculating device will be described below.In this example, a position calculating device that calculates aposition of a moving object using the position of the moving object as astate parameter will be described as an example of the moving statecalculating device. Here, an example to which the invention can beapplied is not limited to the following example.

2-1. Configuration

FIG. 4 is a block diagram illustrating an example of a functionalconfiguration of a position calculating device 1 according to thisexample. The position calculating device 1 includes a processing unit10, a satellite signal receiving unit 20, a sensor unit 30, amanipulation unit 40, a display unit 50, a sound output unit 60, acommunication unit 70, a clock unit 80, and a storage unit 90.

The processing unit 10 is a processor comprehensively controlling theunits of the position calculating device 1 in accordance with variousprograms such as a system program stored in the storage unit 90 and isconstructed by a processor such as a central processing unit (CPU) or adigital signal processor (DSP).

In this example, the processing unit 10 includes a position estimatingunit 11, a constraint condition setting unit 13, and a positioncorrecting unit 15 as principal functional units. These functional unitsare only described as examples and the invention does not necessarilyemploy these functional units. Other functional units may be employed asessential constituents.

The position estimating unit 11 estimates a position of a moving objecthaving the position calculating device 1 installed therein by performinga position estimating process, for example, the Kalman filter.

The constraint condition setting unit 13 sets a constraint condition fornewly calculating a position using the position estimated by theposition estimating unit 11 and a previously-calculated position.

The position correcting unit 15 calculates a new position by correctingthe position estimated by the position estimating unit 11 using theconstraint condition set by the constraint condition setting unit 13.

The satellite signal receiving unit 20 is a receiving device thatreceives GPS satellite signals transmitted from GPS satellites which area kind of positioning satellites. The satellite signal receiving unit 20captures GPS satellite signals by performing a signal process on RF(Radio Frequency) signals received via a GPS antenna not shown, andoperates and acquires various quantities (hereinafter, referred to as“measurement information”) on the captured GPS satellite signals. Themeasurement information includes quantities such as code phases orDoppler frequencies of the GPS satellite signals or quantities such aspseudo-distances or pseudo-distance variations between the positioncalculating device 1 and the GPS satellites. The satellite signalreceiving unit 20 outputs the acquired measurement information to theprocessing unit 10.

The sensor unit 30 is a sensor unit including an inertia sensor such asan acceleration sensor or a gyro sensor. The sensor unit 30 outputs thedetection result thereof to the processing unit 10.

The manipulation unit 40 is an input device including, for example, atouch panel or button switches and outputs the signal of a pressed keyor button to the processing unit 10. Various instructions such as aposition calculation request are input through the manipulation of themanipulation unit 40.

The display unit 50 is a display device including a liquid crystaldisplay (LCD) and performs various displays based on display signalsoutput from the processing unit 10. Position calculation results, timeinformation, and the like are displayed on the display unit 50.

The sound output unit 60 is a sound output device including a speaker orthe like and outputs various sounds based on sound output signals outputfrom the processing unit 10. Voice or music associated with variousapplications is output from the sound output unit 60.

The clock unit 80 is an internal clock of the position calculatingdevice 1 and includes a crystal oscillator having a quartz resonator andan oscillation circuit. The counted time of the clock unit 80 is outputto the processing unit 10 from time to time.

The storage unit 90 includes a storage device such as a read only memory(ROM), a flash ROM, and a random access memory (RAM) and stores a systemprogram for causing the processing unit 10 to control the positioncalculating device or various programs or data used to perform variousapplication processes.

The storage unit 90 stores a position calculating program 91 which isread and performed as the position calculating process (see FIG. 5) bythe processing unit 10. The position calculating program 91 includes aposition estimating program 911 which is executed as the positionestimating process and a position correcting program 913 which isexecuted as the position correcting process as sub routines.

The storage unit 90 also stores adaptation parameter setting data 93,measurement data 95, and calculated position data 97.

The adaptation parameter setting data 93 is data used to set theadaptation parameter S_(k). For example, data for determining thefunction associated with the calculation of the adaptation parameterS_(k) in the rectilinear moving condition shown in FIG. 2B and data fordetermining the function associated with the calculation of theadaptation parameter S_(k) in the smoothing condition shown in FIG. 3Bare included therein.

The measurement data 95 is data in which measurement information outputfrom the satellite signal receiving unit 20 is stored. The measurementinformation is used to perform an operation of correcting a position inthe position estimating process.

The calculated position data 97 is data in which the position calculatedas the final calculated position by the position correcting unit 15 isstored.

2-2. Process Flow

FIG. 5 is a flowchart illustrating the flow of the position calculatingprocess which is performed by the processing unit 10 in accordance withthe position calculating program 91 stored in the storage unit 90. Inthis process, it is assumed that the capturing of the GPS satellitesignals and the acquiring of the measurement information are frequentlyperformed in the satellite signal receiving unit 20 and the measurementinformation can be frequently acquired by the processing unit 10.

First, the processing unit 10 performs an initial setting operation(step B1). Specifically, the position p_(k) of the moving object isdefined as a moving state x_(k) (x_(k)=p_(k)) and a given initialposition p₀ is set as the initial value x₀. For example, an approximateposition of the position calculating device 1 can be set as the initialposition p₀ by causing a user to input the approximate position usingthe manipulation unit 40.

Subsequently, the position estimating unit 11 performs the positionestimating process in accordance with the position estimating program911 stored in the storage unit 90 (step B3). In the position estimatingprocess, the position estimating unit 11 performs a KF predictionoperation (step B5). Specifically, a prediction operation of predictinga current position p_(k) from the previous position p_(k-1) is performedusing a known prediction operational expression of the Kalman filter.

Thereafter, the position estimating unit 11 acquires measurementinformation (such as a code phase or a pseudo-distance) as anobservation value from the satellite signal receiving unit 20 and storesthe acquired measurement information in the measurement data 95 of thestorage unit 90 (step B7).

Subsequently, the position estimating unit 11 performs a KF correctionoperation (step B9). Specifically, a correction operation of correctingthe position p_(k) predicted in step B5 on the basis of a knowncorrection operational expression of the Kalman filter using theobservation value acquired in step B7.

Thereafter, the processing unit 10 performs a moving tendencydetermining process (step B11). Specifically, it is determined whetherthe moving object has a rectilinear moving tendency, a turning tendency,or another tendency, for example, using an acceleration detected by theacceleration sensor or an angular velocity detected by the gyro sensorin the sensor unit 30.

Subsequently, the processing unit 10 performs the position correctingprocess in accordance with the position correcting program 913 stored inthe storage unit 90 (step B13). In the position correcting process, theconstraint condition setting unit 13 sets the constraint condition onthe basis of the determination result of the moving tendency determiningprocess (step B15).

That is, when it is determined that the moving tendency is therectilinear moving tendency (rectilinear moving tendency in step B15),the constraint condition setting unit 13 sets the rectilinear movingcondition (step B17). In the setting of the rectilinear movingcondition, as described above, the constraint condition model functiong_(k)(•) of Expression 14 is formulated using the position differencep_(k)−p_(k-1) instead of the velocity vector v_(k). The method ofsetting the constraint condition definition value d_(k) or theadaptation parameter S_(k) is the same as described with reference toFIGS. 2A and 2B.

Subsequently, the position correcting unit 15 corrects the positionp_(k) estimated in the position estimating process on the basis ofExpressions 11 to 13 using the rectilinear moving condition set in stepB17 (step B19).

When it is determined that the moving tendency is the turning tendency(turning tendency in step B15), the constraint condition setting unit 13sets the smoothing condition (step B21). In the setting of the smoothingcondition in this case, as described above, the angle θ_(k) ofExpression 15 is determined using the position difference p_(k)−p_(k-1)instead of the velocity vector v_(k) and the constraint condition modelfunction g_(k)(•) is formulated. The method of setting the constraintcondition definition value d_(k) and the adaptation parameter S_(k) isthe same as described with reference to FIGS. 3A and 3B.

Subsequently, the position correcting unit 15 corrects the positionp_(k) estimated in the position estimating process on the basis ofExpressions 11 to 13 using the smoothing condition set in step B21 (stepB23).

When it is determined that the moving tendency is neither therectilinear moving tendency nor the turning tendency (otherwise in stepB15), the position correcting unit 15 does not correct the positionp_(k) and the process proceeds to step B25.

Subsequently, the processing unit 10 outputs the calculated position(step B25). When the position p_(k) is corrected using the constraintcondition, the corrected position is output as the calculated position.When the position p_(k) is not corrected using the constraint condition,the estimated position estimated in the position estimating process isoutput as the calculated position.

Then, the processing unit 10 determines whether the process flow shouldend (step B27), and the process returns to the process of step B3 againwhen it is determined that the process flow should not end (NO in stepB27). When it is determined that the process flow should end (YES instep B27), the position calculating process ends.

2-3. Experiment Result

FIGS. 6A and 7A are diagrams illustrating examples of the result of anexperiment in which a position of a moving object is calculated usingthe position calculating device 1. FIG. 6A illustrates an example of theresult of a position calculation experiment using the rectilinear movingcondition and FIG. 6B illustrates an example of the result of a positioncalculation experiment using the smoothing condition.

In the experiment illustrated in FIG. 6A, an automobile having theposition calculating device 1 installed therein caused to travel along astraight load and the positions calculated by the position calculatingdevice 1 in this case are plotted in time series. The plot of blackcircles represents the calculated positions when the rectilinear movingcondition is applied, and the plot of white circles represents thecalculated positions when the rectilinear moving condition is notapplied. From this experiment result, it can be seen that the calculatedpositions form a meandering trace when the rectilinear moving conditionis not applied, but form a substantially linear trace along the roadwhen the rectilinear moving condition is applied, thereby obtaining thecalculated positions reflecting the rectilinear moving tendency.

In the experiment illustrated in FIG. 6B, an automobile having theposition calculating device 1 installed therein is caused to travelalong a curved load and the positions calculated by the positioncalculating device 1 in this case are plotted in time series. The pointof view of the drawing is the same as FIG. 6A. From this experimentresult, it can be seen that the calculated positions do not form asmooth trace when the smoothing condition is not applied, but form asmoothing trace along the curved road when the smoothing condition isapplied, thereby obtaining the calculated positions reflecting theturning tendency.

2-4. Operational Advantages

In the position calculating device 1, the position estimating unit 11estimates the position of a moving object moving in a movement space.The constraint condition setting unit 13 sets the constraint conditionfor newly calculating the position of the moving object using theposition of the moving object estimated by the position estimating unit11 and the previously-calculated position of the moving object. Theposition correcting unit 15 calculates a new position by correcting theposition using the constraint condition set by the constraint conditionsetting unit 13. The position estimating process can be implemented, forexample, as an estimation process using the Kalman filter. It ispossible to correctly calculate the position of the moving object by theuse of the technique of calculating a new position by correcting theestimated position using a predetermined constraint condition instead ofusing the position estimated in the position estimating process as thefinal calculated position.

As the constraint condition, a condition used to correct the estimatedposition so as to comply with the tendency of the state variation of thepreviously-calculated position is set. For example, when the positionvariation of the previously-calculated position indicates therectilinear moving condition, the condition (rectilinear movingcondition) used to correct the position so as to continue torectilinearly move is set as the constraint condition. When the positionvariation of the previously-calculated position indicates the turningtendency, the condition (smoothing condition) used to correct theposition so as to continue to turn is set as the constraint condition.Accordingly, it is possible to calculate an appropriate positioncomplying with the tendency of the state variation of thepreviously-calculated position.

3. Modification Examples

An example to which the invention can be applied is not limited to theabove-mentioned example but can be appropriately modified withoutdeparting from the concept of the invention. Modification examples ofthe invention will be described below.

3-1. Moving State Estimating Process

The moving state estimating process is not limited to the estimationprocess using a known Kalman filter, but any process can be used as longas it uses a state estimating technique in a state space. For example,processes using filters such as an extended Kalman filter, an unscentedKalman filter, a particle filter, and an H^(∞) filter may be used.

3-2. Moving State

The component of the moving state to be calculated has only to includeany of a position, a velocity, and a moving direction of a movingobject. In this case, elements of the position, the velocity, and themoving direction may be used alone as the component of the moving state,or a combination of plural elements may be used as the component of themoving state.

An example of the position calculating device that calculates theposition of a moving object using the position of the moving object asthe component of the moving state is described in the above-mentionedexample, but a velocity vector calculating device that calculates avelocity vector of a moving object using the velocity and the movingdirection (that is, velocity vector) as the component of the movingstate may be embodied.

3-3. Moving Object

The moving state calculating device can be installed in various movingobjects. Specifically, the moving state calculating device may beinstalled in various moving objects such as a person, a bicycle, anautomobile, a subway train, a ship, and an airplane to calculate themoving state.

3-4. Correction Operation

In the above-mentioned example, it is described that the moving tendencydetermining process of determining a moving tendency of a moving objectis performed, the constraint condition based on the determination resultis set, and the operation of correcting the moving state is performed.However, the moving tendency determining process may be skipped, pluraltypes of constraint conditions may be set at the same time, and theoperation of correcting the moving state may be performed.

In this case, matrices in which functions, definition values, andparameters of plural constraint conditions are packed together may bedefined for the constraint condition model function g_(k)(•), theconstraint condition definition value d_(k), and the adaptationparameter S_(k) which are set for each constraint condition, and thecorrection operation may be performed using the correcting operationalexpressions of Expressions 11 to 13.

Specifically, a matrix expressed by Expression 16 is set as a constraintcondition model functional matrix g_(k)(•) including the model functionsg_(k)(•) of plural constraint conditions.

$\begin{matrix}{{g_{k}\left( x_{k} \right)} = \begin{bmatrix}{g_{k}^{(1)}\left( x_{k} \right)} \\{g_{k}^{(2)}\left( x_{k} \right)} \\\vdots\end{bmatrix}} & (16)\end{matrix}$

Here, the numeral written in a parenthesis of a superscript representsthe number of a constraint condition. That is, g⁽¹⁾ _(k)(•) representsthe constraint condition model function of a first constraint condition(for example, rectilinear moving condition) and g⁽²⁾ _(k)(•) representsthe constraint condition model function of a second constraint condition(for example, smoothing condition).

The matrix expressed by Expression 17 is set as the constraint conditiondefinition value matrix including the constraint condition definitionvalues d_(k) of plural constraint conditions as components.

$\begin{matrix}{d_{k} = \begin{bmatrix}d_{k}^{(1)} \\d_{k}^{(2)} \\\vdots\end{bmatrix}} & (17)\end{matrix}$

Here, d⁽¹⁾ _(k) represents the constraint condition definition value ofthe first constraint condition and d⁽²⁾ _(k) represents the constraintcondition definition value of the second constraint condition.

The matrix expressed by Expression 18 is set as the adaptation parametermatrix including the adaptation parameters S_(k) of plural constraintconditions as components.

$\begin{matrix}{S_{k} = \begin{bmatrix}S_{k}^{(1)} & 0 & \ldots \\0 & S_{k}^{(2)} & \ldots \\\vdots & \vdots & \ddots\end{bmatrix}} & (18)\end{matrix}$

Here, S⁽¹⁾ _(k) represents the adaptation parameter of the firstconstraint condition and S⁽²⁾ _(k) represents the adaptation parameterof the second constraint condition.

3-5. Types of Constraint Conditions

The rectilinear moving condition and the smoothing condition areexemplified as the constraint condition in the above-mentionedembodiment, but the constraint conditions are not limited to theseconditions. For example, a constraint condition for setting the velocityvector (or the difference between position vectors) to zero may be setas the constraint condition when a moving object stops.

3-6. Determination of Moving Tendency of Moving Object

It is determined in the above-mentioned embodiment whether a movingobject has the rectilinear moving tendency, or the turning tendency, orotherwise on the basis of the detection result of the sensor unit 30,but it may be determined whether a moving object has the rectilinearmoving tendency, or the turning tendency, or otherwise on the basis of avariation in the previously-calculated position or thepreviously-calculated velocity without using the sensor unit 30.

3-7. Satellite Positioning System

In the above-mentioned embodiment, the GPS satellite signal receiveremploying the GPS is exemplified as the satellite signal receiver of theposition calculating device, but satellite signal receivers employingother satellite positioning systems such as WAAS (Wide Area AugmentationSystem), QZSS (Quasi Zenith Satellite System), GLONASS (GLObalNAvigation Satellite System), and GALILEO may be used.

What is claimed is:
 1. A moving state calculating method for causing aprocessor to execute computer-readable instructions stored in a memoryso as to calculate moving states of a moving object which moves along acurve, the moving states include: a first moving state that has at leastone of a first position, a first velocity, and a first moving directionat a first current time and that is obtained by using at least one of asatellite signal from a satellite and an inertia output signal from aninertia sensor in the moving object without using constraint conditions;a second moving state that has at least one of a second position, asecond velocity, and a second moving direction at a second current timeand that is obtained by using at least one of the satellite signal fromthe satellite and the inertia output signal from the inertia sensor andby using a first constraint condition of the constraint conditions; anda third moving state that has at least one of a third position, a thirdvelocity, and a third moving direction at a third current time and thatis obtained by using at least one of the satellite signal from thesatellite and the inertia output signal from the inertia sensor and byusing a second constraint condition of the constraint conditions,wherein the first current time is earlier than the second current time,and the second current time is earlier than the third current time, themethod comprising: estimating the first moving state by using at leastone of the satellite signal from the satellite and the inertia outputsignal from the inertia sensor at the first current time; setting athird constraint condition of the constraint conditions based on thefirst, second, and third moving states for obtaining a fourth movingstate of the moving object; and calculating the fourth moving state bycorrecting the first moving state with the third constraint condition,wherein the setting includes: obtaining a reference angular change basedon an angular change based on the second and third moving states;obtaining a latest angular change based on the first and third movingstates; and calculating a parameter corresponding to a moving path ofthe moving object that moves from the first moving state by thereference angular change, wherein when a difference between thereference angular change and the latest angular change is larger, theparameter is larger, and the first moving state is corrected based onthe parameter so that the first moving state is corrected in accordancewith the moving path of the moving object that moves from the firstmoving state by the reference angular change.
 2. A moving statecalculating device that is configured to cause a processor to executecomputer-readable instructions stored in a memory so as to calculatemoving states of a moving object which moves along a curve, the movingstates include: a first moving state that has at least one of a firstposition, a first velocity, and a first moving direction at a firstcurrent time and that is obtained by using at least one of a satellitesignal from a satellite and an inertia output signal from an inertiasensor in the moving object without using constraint conditions; asecond moving state that has at least one of a second position, a secondvelocity, and a second moving direction at a second current time andthat is obtained by using at least one of the satellite signal from thesatellite and the inertia output signal from the inertia sensor and byusing a first constraint condition of the constraint conditions; and athird moving state that has at least one of a third position, a thirdvelocity, and a third moving direction at a third current time and thatis obtained by using at least one of the satellite signal from thesatellite and the inertia output signal from the inertia sensor and byusing a second constraint condition of the constraint conditions,wherein the first current time is earlier than the second current time,and the second current time is earlier than the third current time, themoving state calculating device comprising: the processor configured to:estimate the first moving state by using at least one of the satellitesignal from the satellite and the inertia output signal from the inertiasensor at the first current time; set a third constraint condition ofthe constraint conditions based on the first, second, and third movingstates for obtaining a fourth moving state of the moving object; andcalculate the fourth moving state by correcting the first moving statewith the third constraint condition, wherein to set the third constraintcondition, the processor further configured to: obtain a referenceangular change based on an angular change based on the second and thirdmoving states; obtain a latest angular change based on the first andthird moving states; and calculate a parameter corresponding to a movingpath of the moving object that moves from the first moving state by thereference angular change, wherein when a difference between thereference angular change and the latest angular change is larger, theparameter is larger, and the first moving state is corrected based onthe parameter so that the first moving state is corrected in accordancewith the moving path of the moving object that moves from the firstmoving state by the reference angular change.